﻿#region Copyright Notice

//Copyright © 2007-2011, PARROT SA, all rights reserved. 

//DISCLAIMER 
//The APIs is provided by PARROT and contributors "AS IS" and any express or implied warranties, including, but not limited to, the implied warranties of merchantability 
//and fitness for a particular purpose are disclaimed. In no event shall PARROT and contributors be liable for any direct, indirect, incidental, special, exemplary, or 
//consequential damages (including, but not limited to, procurement of substitute goods or services; loss of use, data, or profits; or business interruption) however 
//caused and on any theory of liability, whether in contract, strict liability, or tort (including negligence or otherwise) arising in any way out of the use of this 
//software, even if advised of the possibility of such damage. 

//Author            : Wilke Jansoone
//Email             : wilke.jansoone@digitude.net
//Publishing date   : 28/11/2010 

//Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions
//are met:
//    - Redistributions of source code must retain the above copyright notice, this list of conditions, the disclaimer and the original author of the source code.
//    - Neither the name of the PixVillage Team, nor the names of its contributors may be used to endorse or promote products derived from this software without 
//      specific prior written permission.

#endregion

#region Imports

using System;
using System.Net;
using System.Net.Sockets;
using System.Threading;
using System.Diagnostics;
using System.Collections.Generic;
using System.Text;
using System.Reflection;
using System.ComponentModel;
using System.Drawing;
using System.IO;
using System.Windows;
using System.Configuration;
using System.Runtime.Remoting;
using System.Windows.Threading;

using System.Windows.Media;
using System.Windows.Media.Imaging;

using Wilke.Interactive.Drone;
using Wilke.Interactive.Drone.Control.Enumerations;
using Wilke.Interactive.Drone.Contracts;
using System.Net.NetworkInformation;

#endregion

namespace Wilke.Interactive.Drone.Control
{
    /// <summary>
    /// This class provides the main functionality to pilot the ARDrone.
    /// </summary>
    public class DroneController : IDroneCommander, IDisposable
    {
        #region Public Events

        /// <summary>
        /// Occurs when the contoller sends trace messages.
        /// </summary>
        public event EventHandler<TraceNotificationEventArgs>           OnNotifyTraceMessage;
        /// <summary>
        /// Occurs when new information on the ARDrone is available. This is handled by a timer with elapses at a configurable interval.
        /// </summary>
        /// <see cref="Wilke.Interactive.Drone.Control.DroneControllerConfiguration.DroneInfoTimerInterval"/>
        public event EventHandler<DroneInfoNotificationEventArgs>       OnNotifyDroneInfoMessage;
        /// <summary>
        /// Occurs when a new VIdeo frame has been captured.
        /// </summary>
        public event EventHandler<VideoNotificationEventArgs>           OnNotifyVideoMessage;
        /// <summary>
        /// Occurs when the ARDrone has received input that alters it's position. This can be used to synchronize sreen output with the Drone's current position.
        /// </summary>
        public event EventHandler<InputNotificationEventArgs>           OnNotifyInputMessage;
        /// <summary>
        /// Occurs when the status of the ARDrone has changed.
        /// </summary>
        public event EventHandler<ConnectionStatusChangedEventArgs>     OnConnectionStatusChanged;
        /// <summary>
        /// Occurs when drone needs to send information about a detected target (e.g. coloured tag).
        /// </summary>
        public event EventHandler<VisionDetectNotificationEventArgs>    OnVisionDetectMessage;

        #endregion

        #region Public Properties

        /// <summary>
        /// Gets or sets the trace notification level. Possible values are verbose, information, warning, error and none.
        /// </summary>
        /// <value>The trace notification level.</value>
        public TraceNotificationLevel TraceNotificationLevel { get; set; }
        /// <summary>
        /// Gets or sets the connection status of the <see cref="DroneController"/>.
        /// </summary>
        /// <value>The connection status.</value>
        public ConnectionStatus ConnectionStatus { get; set; }      
        /// <summary>
        /// Gets or sets a value indicating whether the ARDrone needs to save a still image.
        /// </summary>
        /// <value><c>true</c> if image is requested; otherwise, <c>false</c>.</value>
        public bool PictureRequested { get; set; }
        /// <summary>
        /// Gets or sets a value indicating whether the ARDrone is detecting coloured tags (series of vertical coloured bands).
        /// </summary>
        /// <value><c>true</c> if detection is enabled; otherwise, <c>false</c>.</value>
        public bool  VisionDetectEnabled{ get; set; }
        /// <summary>
        /// Gets or sets a value indicating whether the DroneController is recording video.
        /// </summary>
        /// <value>
        /// 	<c>true</c> if the DroneController is recording video; otherwise, <c>false</c>.
        /// </value>
        public bool DroneIsRecording { get; set; }
        /// <summary>
        /// Gets or sets a value indicating whether the ARDrone is flying.
        /// </summary>
        /// <value><c>true</c> if tte ARdrone is flying; otherwise, <c>false</c>.</value>
        public bool DroneIsFlying { get; set; }
        /// <summary>
        /// The pitch throttle value defined by the <see cref="DroneControllerConfiguration"/> instance.
        /// </summary>
        public Single PitchThrottleValue { get { return DroneControllerConfiguration.PitchThrottle; } }
        /// <summary>
        /// The roll throttle value defined by the <see cref="DroneControllerConfiguration"/> instance.
        /// </summary>
        public Single RollThrottleValue { get { return DroneControllerConfiguration.RollThrottle; } }
        /// <summary>
        /// The height throttle value defined by the <see cref="DroneControllerConfiguration"/> instance.
        /// </summary>
        public Single HeightThrottleValue { get { return DroneControllerConfiguration.HeightThrottle; } }
        /// <summary>
        /// The yaw throttle value defined by the <see cref="DroneControllerConfiguration"/> instance.
        /// </summary>
        public Single YawThrottleValue { get { return DroneControllerConfiguration.YawThrottle; } }

        #endregion

        #region Private Properties

        private DroneControllerConfiguration        DroneControllerConfiguration { get; set; }
        private DroneProxy                          DroneProxy { get; set; }
        
        private VideoImage                          VideoImage { get; set; }
        private VideoFileWriter                     VideoFileWriter { get; set; }
        private DroneInfoNotificationEventArgs      DroneInfoNotificationEventArgs { get; set; }
        private System.Timers.Timer                 DroneInfoTimer { get; set; }
        private string                              VideoFileName { get; set; }
        private string                              PictureFileName { get; set; }       

        #endregion

        #region Construction

        /// <summary>
        /// Initializes a new instance of the <see cref="DroneController"/> class.
        /// </summary>
        public DroneController() : this(new DroneControllerConfiguration()) { }

        /// <summary>
        /// Initializes a new instance of the <see cref="DroneController"/> class.
        /// </summary>
        /// <param name="droneControllerConfiguration">The drone controller configuration.</param>
        public DroneController(DroneControllerConfiguration droneControllerConfiguration)
        {
            #region Initialize DroneController

            DroneControllerConfiguration = droneControllerConfiguration;

            DroneProxy = DroneProxy.Create();

            VideoImage = new VideoImage();
            VideoImage.ImageComplete += new EventHandler<ImageCompleteEventArgs>(VideoImage_ImageComplete);

            DroneInfoNotificationEventArgs = new DroneInfoNotificationEventArgs();
            DroneInfoTimer = new System.Timers.Timer(Constants.DroneInfoTimerInterval);
            DroneInfoTimer.Elapsed += new System.Timers.ElapsedEventHandler(DroneInfoTimer_Elapsed);

            #endregion
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="DroneController"/> class. A new instance of the <see cref=" Wilke.Interactive.Drone.Control.DroneControllerConfiguration"/> class is used to configure this <see cref="DroneController"/> instance. Except from the network identifier all properties are kept to their default values.
        /// </summary>
        /// <param name="networkIdentifier">The name of the adhoc network created by the ARDrone.</param>
        public DroneController(String networkIdentifier)
        {
            #region Initialize DroneController

            DroneControllerConfiguration = new DroneControllerConfiguration();
            DroneControllerConfiguration.NetworkIdentifier = networkIdentifier;

            DroneProxy = DroneProxy.Create();

            VideoImage = new VideoImage();
            VideoImage.ImageComplete += new EventHandler<ImageCompleteEventArgs>(VideoImage_ImageComplete);

            DroneInfoNotificationEventArgs = new DroneInfoNotificationEventArgs();
            DroneInfoTimer = new System.Timers.Timer(Constants.DroneInfoTimerInterval);
            DroneInfoTimer.Elapsed += new System.Timers.ElapsedEventHandler(DroneInfoTimer_Elapsed);

            #endregion
        }

        #endregion

        #region Public Methods

        #region IDroneCommander Implementation

        /// <summary>
        /// Starts the engines of the ARDrone. The ARDrone will take of and start hovering at a predetermined height.
        /// </summary>
        public void StartEngines()
        {
            DroneProxy.StartEngines();
            DroneIsFlying = true;
        }
        /// <summary>
        /// Stops the ARDrone engines, the ARDrone will land smoothly.
        /// </summary>
        public void StopEngines()
        {
            DroneProxy.StopEngines();
            DroneIsFlying = false;
        }
        /// <summary>
        /// Starts issuing an emergency reset command to the ARDrone. The engines will stop immediately and ARDrone stops hovering. In most cases this will lead to a crash.
        /// </summary>
        public void StartReset()
        {
            DroneProxy.StartResetDrone();
        }
        /// <summary>
        /// Stops issuing an emergency reset command to the ARDrone.
        /// </summary>
        public void StopReset()
        {
            DroneProxy.StopResetDrone();
        }
        /// <summary>
        /// Makes the ARDrone animate its LED's.
        /// </summary>
        /// <param name="id">The id of the animation to play.</param>
        /// <param name="frequency">The blink frequency of the animation in times per second (Hz).</param>
        /// <param name="duration">The duration of the animation in seconds.</param>
        public void PlayLedAnimation(int id, int frequency, int duration)
        {
            CommandCenter.PlayLedAnimation((LedAnimation) id, frequency, duration);
        }       
        /// <summary>
        /// Sets the flat trim. Has to be called before each new flight.
        /// </summary>
        public void SetFlatTrim()
        {
            CommandCenter.SetFlatTrim();
        }
        /// <summary>
        /// Issues a command to the ARDrone to start detecting predefined coloured patterns (orrange|yellow|orange, orange|green|orange and orange|blue|orange).
        /// </summary>
        public void StartVisionDetect()
        {
            CommandCenter.StartVisonDetect();
            VisionDetectEnabled = true;
        }
        /// <summary>
        /// Issues a command to the ARDrone to stop detecting predefined coloured patterns.
        /// </summary>
        public void StopVisionDetect()
        {
            CommandCenter.StopVisonDetect();
            VisionDetectEnabled = false;
        }
        /// <summary>
        /// Displays the next video channel.
        /// </summary>
        public void DisplayNextVideoChannel()
        {
            CommandCenter.SwitchVideoChannel(VideoChannel.Next);
        }
        /// <summary>
        /// Switches to the selected video channel.
        /// </summary>
        public void SwitchVideoChannel(int videoChannel)
        {
            CommandCenter.SwitchVideoChannel((VideoChannel) videoChannel);
        }
        /// <summary>
        /// Sets the flight parameters. This allows to pilot the ARDrone.
        /// </summary>
        /// <param name="roll">The roll parameter (Tilt Left/Right - Phi angle).</param>
        /// <param name="pitch">The pitch parameter (Tilt Front/Back - Theta angle)</param>
        /// <param name="height">The height parameter. (Move Up/Down)</param>
        /// <param name="yaw">The yaw parameter. (Rotate Left/Right - Psi angle)</param>
        /// <remarks>All parameters have a value between -1 and 1.</remarks>
        public void SetFlightParameters(float roll, float pitch, float height, float yaw)
        {
            CommandCenter.SetProgressiveInputValues(roll, pitch, height, yaw);

            if (DroneControllerConfiguration.EnableInputFeedback)
            {
                OnFlightParametersChanged(roll, pitch, height, yaw);
            }
        }
        /// <summary>
        /// Starts the recording of video images.
        /// </summary>        
        public bool StartRecordVideo()
        {
            return StartRecordVideo(DroneControllerConfiguration.VideoFilePath);
        }
        /// <summary>
        /// Stops the recording of video images.
        /// </summary>
        public void StopRecordVideo()
        {
            DroneIsRecording = false;
            if (VideoFileWriter != null)
            {
                VideoFileWriter.Dispose();
                VideoFileWriter = null;
            }
        }

        /// <summary>
        /// Saves the current image as a snapshot.
        /// </summary>
        /// <returns>
        /// <c>true</c> if path where pictures are stored exists; otherwise, <c>false</c>.
        /// </returns>
        public bool TakePicture()
        {
            return TakePicture(DroneControllerConfiguration.PictureFilePath);
        }
        /// <summary>
        /// Saves the current image as a snapshot.
        /// </summary>
        /// <param name="path">The path where to save the snapshot.</param>
        /// <returns><c>true</c> if path exists; otherwise, <c>false</c>.</returns>
        /// <remarks>This method checks whether the path exists and if not it skips the recording and returns false. I can not throw an exception because if the ARDrone is in flight and
        /// a request to take a picture is made the application would hang and leave the ARDrone uncontrolled.
        /// It is up to the consumer of the DroneController to provide better checking with more feedback.
        /// </remarks>
        public bool TakePicture(string path)
        {
            bool pathExists = Directory.Exists(path);

            if (pathExists)
            {
                PictureFileName = CommonHelper.GenerateNewNumberedFileName(path, Constants.DefaultPictureFileNamePattern, "jpeg");
                PictureRequested = true;
            }

            return pathExists;
        }  

        #endregion

        /// <summary>
        /// Makes the ARDrone animate its LED's.
        /// </summary>
        /// <param name="ledAnimation">The animation to play.</param>
        /// <param name="frequency">The blink frequency of the animation in times per second (Hz).</param>
        /// <param name="duration">The duration of the animation in seconds.</param>
        public void PlayLedAnimation(LedAnimation ledAnimation, int frequency, int duration)
        {
            CommandCenter.PlayLedAnimation(ledAnimation, frequency, duration);
        }

        /// <summary>
        /// Starts a video recording session.
        /// </summary>
        /// <param name="path">The full path of video file that needs to be created.</param>
        /// <returns><c>true</c> if path exists; otherwise, <c>false</c>.</returns>
        /// <remarks>This method checks whether the path exists and if not it skips the recording and returns false. I can not throw an exception because if the ARDrone is in flight and
        /// a request to record video is made the application would hang and leave the ARDrone uncontrolled.
        /// It is up to the consumer of the DroneController to provide better checking with more feedback.
        /// </remarks>
        public bool StartRecordVideo(string path)
        {
            bool pathExists = Directory.Exists(path);

            if (pathExists)
            {
                VideoFileName = CommonHelper.GenerateNewNumberedFileName(path, Constants.DefaultVideoFileNamePattern, "wmv");
                DroneIsRecording = true;
            }

            return pathExists;
        }

        /// <summary>
        /// Switches the video channel. The ARDrone has two cameras, a horizontal and a vertical one. 
        /// Calling this method with the appropriate parameter results in images from either one of the cameras
        /// or a composite image from both cameras at the same time.
        /// </summary>
        /// <param name="videoChannel">An enumerated value indicating what kind of image to produce..</param>
        public void SwitchVideoChannel(VideoChannel videoChannel)
        {
            CommandCenter.SwitchVideoChannel(videoChannel);
        }

        /// <summary>
        /// This method establishes a connection with the ARDrone.
        /// </summary>
        /// <remarks>
        /// In order to be able to provide real time feedback to the requester this call is asynchronous. 
        /// Feedback is provided via the 'OnNotifyTraceMessage' event.
        /// </remarks>
        public void Connect()
        {
            BackgroundWorker backgroundWorker = new BackgroundWorker();
            backgroundWorker.DoWork += new DoWorkEventHandler(ConnectAsync);
            backgroundWorker.RunWorkerAsync();
        }

        /// <summary>
        /// This method tears down the connection with the ARDrone.
        /// </summary>
        public void Disconnect()
        {
            try
            {
                DroneInfoTimer.Stop();
                StopEngines();
                CommunicationCenter.Disconnect();
                ConnectionStatus = ConnectionStatus.Closed;
            }
            catch (DroneException droneException)
            {
                OnNotifyTraceMessage(this, new TraceNotificationEventArgs(droneException.Message, droneException.NotificationSource, TraceNotificationLevel.Error));
            }
            catch (Exception exception)
            {
                OnNotifyTraceMessage(this, new TraceNotificationEventArgs(exception.Message, NotificationSource.DroneController, TraceNotificationLevel.Error));
            }
            finally
            {
                if (ConnectionStatus != ConnectionStatus.Closed)
                {
                    ConnectionStatus = ConnectionStatus.Error;
                }
            }
        }

        /// <summary>
        /// In order to receive the drone onboard configuration this method needs to be called. The response will be sent via a TCP channel (e.g. port 5555).
        /// The Communication center contains the logic to receive TCP frames on this control channel. The response will be passed via the 'OnNotifyDroneInfoMessage'
        /// evcent.
        /// </summary>
        public void RequestConfiguredValues()
        {
            CommandCenter.RequestConfiguredValues();
        }

        /// <summary>
        /// Sets the ARDrone configuration for outdoor flights. These parameters can be adapted according to personal requirements.
        /// </summary>
        /// <remarks>
        /// More information can be found in developers guide, chapter 'Drone Configuration'.
        /// </remarks>
        public void SetOutdoorConfiguration()
        {
            SetConfiguration("control:outdoor", "TRUE");
            SetConfiguration("control:flight_without_shell", "FALSE");

            SetConfiguration("control:altitude_max", "2000");
            SetConfiguration("control:euler_angle_max", ".25");
            SetConfiguration("detect:enemy_colors", "1");
        }

        /// <summary>
        /// Sets the ARDrone configuration for indoor flights. These parameters can be adapted according to personal requirements.
        /// </summary>
        /// <remarks>
        /// More information can be found in developers guide, chapter 'Drone Configuration'.
        /// </remarks>
        public void SetIndoorConfiguration()
        {
            SetConfiguration("control:outdoor", "FALSE");
            SetConfiguration("control:flight_without_shell", "FALSE");            
            SetConfiguration("control:altitude_max", "2000");
            SetConfiguration("control:euler_angle_max", ".25");
            SetConfiguration("detect:enemy_colors", "1");
        }

        /// <summary>
        /// Sets ARDrone configuration values.
        /// </summary>
        /// <remarks>
        /// More information can be found in developers guide, chapter 'Drone Configuration'.
        /// </remarks>
        public void SetConfiguration(string parameterName, string parameterValue)
        {
            CommandCenter.SetConfiguration(parameterName, parameterValue);
        }

        /// <summary>
        /// Performs application-defined tasks associated with freeing, releasing, or resetting unmanaged resources.
        /// </summary>
        public void Dispose()
        {
            DroneInfoTimer.Stop();
            DroneInfoTimer.Dispose();
        }

        #endregion

        #region Private Methods

        private void ConnectAsync(object sender, DoWorkEventArgs e)
        {
            try
            {
                ConnectionStatus = ConnectionStatus.Initializing;

                OnNotifiedTraceMessage("Initializing DroneController.", NotificationSource.DroneController, TraceNotificationLevel.Information);

                #region Register DroneController

                CommunicationCenter.RegisterController(this);
                CommandCenter.RegisterController(this);

                #endregion

                bool success = CommunicationCenter.Connect();

                if (success)
                {                   
                    DroneInfoTimer.Start();
                    ConnectionStatus = ConnectionStatus.Open;
                    OnNotifiedTraceMessage("DroneController initialized.", NotificationSource.DroneController, TraceNotificationLevel.Information);
                }
            }
            catch (DroneException droneException)
            {
                OnNotifiedTraceMessage(droneException.Message, droneException.NotificationSource, TraceNotificationLevel.Error);
            }
            catch (Exception exception)
            {
                OnNotifiedTraceMessage(exception.Message, NotificationSource.DroneController, TraceNotificationLevel.Error);
            }
            finally
            {
                switch (ConnectionStatus)
                { 
                    case ConnectionStatus.Open:
                        break;
                    case ConnectionStatus.Initializing:
                    case ConnectionStatus.Closed:
                    case ConnectionStatus.CloseRequested:
                        ConnectionStatus = ConnectionStatus.Error;
                        OnNotifiedTraceMessage("DroneController not initialized.", NotificationSource.DroneController, TraceNotificationLevel.Warning);
                        break;
                }

                if (OnConnectionStatusChanged != null)
                {
                    OnConnectionStatusChanged(this, new ConnectionStatusChangedEventArgs(ConnectionStatus));
                }
            }
        }     

        private void UpdateDroneConfigurationValues(string configuredValues)
        {
            DroneProxy.UpdateDroneConfiguration(configuredValues);
        }

        private void SavePicture()
        {
            BitmapEncoder encoder = new JpegBitmapEncoder();

            using (Stream fileStream = new FileStream(PictureFileName, FileMode.CreateNew))
            {
                encoder.Frames.Add(BitmapFrame.Create(VideoImage.ImageSource));
                encoder.Save(fileStream);
            }           
        }

        #endregion

        #region EventHandlers

        private int traceThrottler = 0;
        private int inputNotificationThrottler = 0;
        private int visionDetectNotificationThrottler = 0;

        void DroneInfoTimer_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
        {
            #region Collect DroneInfo

            DroneInfoNotificationEventArgs.BatteryLevel = DroneProxy.BatteryLevel;
            DroneInfoNotificationEventArgs.DroneStatus = DroneProxy.DroneStatus;
            DroneInfoNotificationEventArgs.Height = DroneProxy.Height;
            DroneInfoNotificationEventArgs.Theta = DroneProxy.Theta;
            DroneInfoNotificationEventArgs.Phi = DroneProxy.Phi;
            DroneInfoNotificationEventArgs.Psi = DroneProxy.Psi;

            DroneInfoNotificationEventArgs.WorkerThreadATCommandsActive     = CommunicationCenter.WorkerThreadATCommandsActive;
            DroneInfoNotificationEventArgs.WorkerThreadNavigationDataActive = CommunicationCenter.WorkerThreadNavigationDataActive;
            DroneInfoNotificationEventArgs.WorkerThreadVideoStreamActive    = CommunicationCenter.WorkerThreadVideoStreamActive;
            DroneInfoNotificationEventArgs.WorkerThreadControlInfoActive    = CommunicationCenter.WorkerThreadControlInfoActive;

            DroneInfoNotificationEventArgs.DroneConfiguration = DroneProxy.DroneConfiguration;

            #endregion

            OnNotifiedDroneInfo(DroneInfoNotificationEventArgs);
        }

        private void VideoImage_ImageComplete(object sender, ImageCompleteEventArgs e)
        {
            if (OnNotifyVideoMessage != null && ConnectionStatus != ConnectionStatus.Closed)
            {
                OnNotifyVideoMessage(this, new VideoNotificationEventArgs(VideoImage.ImageSource));

                if (DroneIsRecording)
                {
                    if (VideoFileWriter == null)
                    {
                        //VideFileWriter must be created on the same thread as the one that
                        //writes to it.
                        VideoFileWriter = new VideoFileWriter(VideoFileName, VideoQuality.Kbps256);
                    }

                    WriteableBitmap videoImage = e.ImageSource as WriteableBitmap;

                    using (Bitmap originalVideoBitmap = new Bitmap((int)videoImage.Width, (int)videoImage.Height, videoImage.BackBufferStride, System.Drawing.Imaging.PixelFormat.Format16bppRgb565, videoImage.BackBuffer))
                    {
                        using (Bitmap newVideoBitmap = new Bitmap(originalVideoBitmap))
                        {
                            VideoFileWriter.AddFrame(newVideoBitmap);
                        }
                    }
                }

                if (PictureRequested)
                {
                    SavePicture();
                    PictureRequested = false;
                }
            }
        }

        private void OnNotifiedVisionDetect(VisionDetectNotificationEventArgs e)
        {
            if (OnVisionDetectMessage != null && ConnectionStatus != ConnectionStatus.Closed)
            {
                if (visionDetectNotificationThrottler > 5)
                {
                    OnVisionDetectMessage(this, e);
                    visionDetectNotificationThrottler = 0;
                }

                visionDetectNotificationThrottler++;
            }
        }

        private void OnNotifiedDroneInfo(DroneInfoNotificationEventArgs e)
        {
            if (OnNotifyTraceMessage != null && ConnectionStatus != ConnectionStatus.Closed)
            {
                OnNotifyDroneInfoMessage(this, e);
            }
        }

        private void OnNotifiedTraceMessage(TraceNotificationEventArgs e)
        {
            if (OnNotifyTraceMessage != null && ConnectionStatus != ConnectionStatus.Closed)
            {
                if (e.NotificationLevel >= TraceNotificationLevel)
                {
                    //Verbose notification will be Throttled in order
                    //to avoid message flooding in the client
                    if (e.NotificationLevel == TraceNotificationLevel.Verbose)
                    {
                        traceThrottler++;

                        if (traceThrottler > 10)
                        {
                            OnNotifyTraceMessage(this, e);
                            traceThrottler = 0;
                        }
                    }
                    else
                    {
                        OnNotifyTraceMessage(this, e);
                    }
                }
            }
        }

        private void OnNotifiedTraceMessage(string notificationMessage, NotificationSource notificationSource, TraceNotificationLevel notificationLevel)
        {
            OnNotifiedTraceMessage(new TraceNotificationEventArgs(notificationMessage, notificationSource, notificationLevel));
        }

        private void OnNotifiedTraceMessage(string notificationMessage, Exception exception, NotificationSource notificationSource, TraceNotificationLevel notificationLevel)
        {
            string notificationMessageFinal = String.Format("{0} Error message is: '{1}'.", notificationMessage, exception.Message);

            OnNotifiedTraceMessage(new TraceNotificationEventArgs(notificationMessageFinal, notificationSource, notificationLevel));
        }

        private void OnFlightParametersChanged(Single roll, Single pitch, Single height, Single yaw)
        {
            if (OnNotifyInputMessage != null)
            {
                if (inputNotificationThrottler > 1)
                {
                    OnNotifyInputMessage(this, new InputNotificationEventArgs(roll, pitch, height, yaw));
                    inputNotificationThrottler = 0;
                }

                inputNotificationThrottler++;
            }
        }

        #endregion

        #region Nested Classes

        private static class CommunicationCenter
        {
            #region Private Properties

            private static DroneController                              DroneController { get; set; }
            private static DroneProxy                                   DroneProxy { get; set; }

            private static Thread                                       WorkerThreadReceiveNavigationData { get; set; }
            private static Thread                                       WorkerThreadReceiveControlInfo { get; set; }
            private static Thread                                       WorkerThreadReceiveVideoStream { get; set; }
            private static Thread                                       WorkerThreadSendATCommands { get; set; }

            private static UInt32                                       LocalMessageIndex { get; set; }

            private static ManualResetEvent                             WaitHandle { get; set; }
            private static string                                       LocalIPAddress { get; set; }

            private static Dictionary<CommunicationChannel, UdpClient>  UdpSockets { get; set; }
            private static Dictionary<CommunicationChannel, TcpClient>  TcpSockets { get; set; }
            private static Dictionary<CommunicationChannel, IPEndPoint> EndPoints { get; set; }

            #endregion

            #region Public Properties

            public static bool WorkerThreadVideoStreamActive { get; private set; }
            public static bool WorkerThreadATCommandsActive { get; private set; }
            public static bool WorkerThreadNavigationDataActive { get; private set; }
            public static bool WorkerThreadControlInfoActive { get; private set; }

            #endregion

            #region Construction

            static CommunicationCenter()
            {
                UdpSockets  = new Dictionary<CommunicationChannel, UdpClient>();
                TcpSockets  = new Dictionary<CommunicationChannel, TcpClient>();
                EndPoints   = new Dictionary<CommunicationChannel, IPEndPoint>();
            }

            #endregion

            #region Internal Methods

            internal static void RegisterController(DroneController droneController)
            {
                DroneController = droneController;
                DroneProxy = droneController.DroneProxy;
            }

            internal static bool Connect()
            {
                bool continueProcess = false;

                try
                {
                    if (DroneController.DroneControllerConfiguration.EnableATCommandSimulation)
                    {
                        LocalIPAddress = "127.0.0.1";
                        continueProcess = true;
                    }
                    else
                    {
                        #region Connect To AdHoc Wireless Network

                        string networkIdentifier = DroneController.DroneControllerConfiguration.NetworkIdentifier;

                        WlanInterface wlanInterface = NetworkHelper.GetWirelessNetworkInterface();

                        if (wlanInterface != null)
                        {
                            foreach (Wlan.WlanAvailableNetwork availableNetwork in wlanInterface.GetAvailableNetworkList(Wlan.WlanGetAvailableNetworkFlags.IncludeAllAdhocProfiles))
                            {
                                string availableNetworkName = CommonHelper.ConvertByteArrayToString(availableNetwork.dot11Ssid.SSID, (int)availableNetwork.dot11Ssid.SSIDLength);

                                if (availableNetworkName == networkIdentifier)
                                {
                                    continueProcess = true;
                                    break;
                                }
                            }

                            if (!continueProcess)
                            {
                                DroneController.OnNotifiedTraceMessage(String.Format("Could not find drone wireless network with network id '{0}'.", networkIdentifier), NotificationSource.CommunicationCenter, TraceNotificationLevel.Error);
                            }
                            else
                            {
                                wlanInterface.WlanNotification += new WlanInterface.WlanNotificationEventHandler(wlanInterface_WlanNotification);

                                WaitHandle = new ManualResetEvent(false);

                                Wlan.Dot11Ssid ssid = new Wlan.Dot11Ssid();
                                ssid.SSID = CommonHelper.ConvertStringToByteArray(networkIdentifier);
                                ssid.SSIDLength = (uint)networkIdentifier.Length;
                                wlanInterface.Connect(Wlan.WlanConnectionMode.DiscoveryUnsecure, Wlan.Dot11BssType.Independent, ssid, Wlan.WlanConnectionFlags.AdhocJoinOnly);

                                WaitHandle.WaitOne();

                                if (wlanInterface.InterfaceState != Wlan.WlanInterfaceState.Connected)
                                {
                                    continueProcess = false;
                                }
                                else
                                {
                                    try
                                    {
                                        //Canged code in order to take the last unicast address if there are multiple addresses and if only one it takes the default.
                                        //See workitem 521.
                                        IPInterfaceProperties interfaceProperties = wlanInterface.NetworkInterface.GetIPProperties();
                                        LocalIPAddress = interfaceProperties.UnicastAddresses[interfaceProperties.UnicastAddresses.Count - 1].Address.ToString();
                                    }
                                    catch (Exception)
                                    {
                                        DroneController.OnNotifiedTraceMessage("Could not retrieve guest IP address. Connect interrupted.", NotificationSource.DroneController, TraceNotificationLevel.Error);
                                        continueProcess = false;
                                    }
                                }
                            }
                        }
                        else
                        {
                            LocalIPAddress = DroneController.DroneControllerConfiguration.GuestIpAddress;
                        }

                        #endregion
                    }

                    continueProcess = true;

                    if (continueProcess)
                    {
                        #region Create Communication Channels and Worker Threads

                        #region AT commands

                        if (DroneController.DroneControllerConfiguration.EnableATCommandThread)
                        {
                            DroneController.OnNotifiedTraceMessage("Setting up AT command communication channel.", NotificationSource.CommunicationCenter, TraceNotificationLevel.Information);

                            UdpSockets[CommunicationChannel.Command] = NetworkHelper.CreateUdpSocket(LocalIPAddress, DroneController.DroneControllerConfiguration.CommandPort, Constants.SocketTimeoutATCommand);
                            EndPoints[CommunicationChannel.Command] = NetworkHelper.CreateRemoteEndPoint(DroneController.DroneControllerConfiguration.DroneIpAddress, DroneController.DroneControllerConfiguration.CommandPort);

                            WorkerThreadSendATCommands = new Thread(new ThreadStart(ThreadMethodSendATCommands));
                            WorkerThreadSendATCommands.Name = "ATCommandThread";
                            WorkerThreadSendATCommands.Start();

                            DroneController.OnNotifiedTraceMessage("AT command worker thread started.", NotificationSource.CommunicationCenter, TraceNotificationLevel.Information);
                        }
                        else
                        {
                            DroneController.OnNotifiedTraceMessage("ATCommand communication not enabled.", NotificationSource.DroneController, TraceNotificationLevel.Warning);
                        }

                        #endregion

                        #region Navigation Data

                        if (DroneController.DroneControllerConfiguration.EnableNavigationDataThread)
                        {
                            DroneController.OnNotifiedTraceMessage("Setting up Navigation Data communication channel.", NotificationSource.CommunicationCenter, TraceNotificationLevel.Information);

                            UdpSockets[CommunicationChannel.NavigationData] = NetworkHelper.CreateUdpSocket(LocalIPAddress, DroneController.DroneControllerConfiguration.NavigationDataPort, Constants.SocketTimeoutNavigationData);
                            EndPoints[CommunicationChannel.NavigationData] = NetworkHelper.CreateRemoteEndPoint(DroneController.DroneControllerConfiguration.DroneIpAddress, DroneController.DroneControllerConfiguration.NavigationDataPort);

                            WorkerThreadReceiveNavigationData = new Thread(new ThreadStart(ThreadMethodReceiveNavigationData));
                            WorkerThreadReceiveNavigationData.Name = "NavigationDataThread";
                            WorkerThreadReceiveNavigationData.Start();

                            DroneController.OnNotifiedTraceMessage("Navigation Data worker thread started.", NotificationSource.DroneController, TraceNotificationLevel.Information);
                        }
                        else
                        {
                            DroneController.OnNotifiedTraceMessage("Navigation Data communication not enabled.", NotificationSource.DroneController, TraceNotificationLevel.Warning);
                        }

                        #endregion

                        #region Video Stream

                        if (DroneController.DroneControllerConfiguration.EnableVideoStreamThread)
                        {
                            DroneController.OnNotifiedTraceMessage("Setting up Video Stream communication channel.", NotificationSource.CommunicationCenter, TraceNotificationLevel.Information);

                            UdpSockets[CommunicationChannel.VideoStream] = NetworkHelper.CreateUdpSocket(LocalIPAddress, DroneController.DroneControllerConfiguration.VideoStreamPort, Constants.SocketTimeoutVideoStream);
                            EndPoints[CommunicationChannel.VideoStream] = NetworkHelper.CreateRemoteEndPoint(DroneController.DroneControllerConfiguration.DroneIpAddress, DroneController.DroneControllerConfiguration.VideoStreamPort);

                            WorkerThreadReceiveVideoStream = new Thread(new ThreadStart(ThreadMethodReceiveVideoStream));
                            WorkerThreadReceiveVideoStream.Name = "VideoStreamThread";
                            WorkerThreadReceiveVideoStream.Start();

                            DroneController.OnNotifiedTraceMessage("Video Stream worker thread started.", NotificationSource.DroneController, TraceNotificationLevel.Information);
                        }
                        else
                        {
                            DroneController.OnNotifiedTraceMessage("Video Stream communication not enabled.", NotificationSource.DroneController, TraceNotificationLevel.Warning);
                        }

                        #endregion

                        #region Control Info

                        if (DroneController.DroneControllerConfiguration.EnableControlInfoThread)
                        {
                            DroneController.OnNotifiedTraceMessage("Setting up Control Info communication channel.", NotificationSource.CommunicationCenter, TraceNotificationLevel.Information);

                            TcpSockets[CommunicationChannel.ControlInfo] = NetworkHelper.CreateTcpSocket(LocalIPAddress, DroneController.DroneControllerConfiguration.ControlInfoPort, Constants.SocketTimeoutControlInfo);
                            EndPoints[CommunicationChannel.ControlInfo] = NetworkHelper.CreateRemoteEndPoint(DroneController.DroneControllerConfiguration.DroneIpAddress, DroneController.DroneControllerConfiguration.ControlInfoPort);

                            WorkerThreadReceiveControlInfo = new Thread(new ThreadStart(ThreadMethodReceiveControlInfo));
                            WorkerThreadReceiveControlInfo.Name = "ControlInfoThread";
                            WorkerThreadReceiveControlInfo.Start();

                            DroneController.OnNotifiedTraceMessage("Control Info worker thread started.", NotificationSource.DroneController, TraceNotificationLevel.Information);
                        }
                        else
                        {
                            DroneController.OnNotifiedTraceMessage("Control Info communication not enabled.", NotificationSource.DroneController, TraceNotificationLevel.Warning);
                        }

                        #endregion

                        #endregion
                    }

                    return continueProcess;
                }
                catch (Exception exception)
                {
                    throw new DroneException(NotificationSource.CommunicationCenter, exception.Message);
                }
            }

            internal static void Disconnect()
            {
                try
                {
                    if (DroneController.ConnectionStatus == ConnectionStatus.Open)
                    {
                        WorkerThreadSendATCommands = null;
                        WorkerThreadReceiveNavigationData = null;
                        WorkerThreadReceiveVideoStream = null;

                        DroneController.ConnectionStatus = ConnectionStatus.Closed;

                        foreach (KeyValuePair<CommunicationChannel, UdpClient> entry in UdpSockets)
                        {
                            NetworkHelper.CloseUdpConnection(entry.Value);
                        }

                        foreach (KeyValuePair<CommunicationChannel, TcpClient> entry in TcpSockets)
                        {
                            NetworkHelper.CloseTcpConnection(entry.Value);
                        }

                        CommandCenter.ResetCommandBatchSequence();

                        WorkerThreadSendATCommands = null;
                        WorkerThreadReceiveNavigationData = null;
                        WorkerThreadReceiveVideoStream = null;
                        WorkerThreadReceiveControlInfo = null;
                    }
                }
                catch (Exception exception)
                {
                    throw new DroneException(NotificationSource.CommunicationCenter, exception.Message);
                }
            }

            #endregion

            #region Event Handlers

            private static void wlanInterface_WlanNotification(Wlan.WlanNotificationData notifyData)
            {
                DroneController.OnNotifiedTraceMessage(notifyData.NotificationCode.ToString(), NotificationSource.CommunicationCenter, TraceNotificationLevel.Information);

                if (notifyData.NotificationCode.ToString() == "ConnectionComplete")
                {
                    WaitHandle.Set();
                }
            }

            #endregion

            #region Private Methods

            private static void SendMessage(CommunicationChannel communicationChannel, int message)
            {
                try
                {
                    SendMessage(communicationChannel, BitConverter.GetBytes(message));
                }
                catch { }
            }

            private static void SendMessage(CommunicationChannel communicationChannel, string message)
            {
                try
                {
                    byte[] buffer = Encoding.ASCII.GetBytes(message);
                    SendMessage(communicationChannel, buffer);
                }
                catch { }
            }

            private static void SendMessage(CommunicationChannel communicationChannel, byte[] message)
            {
                try
                {
                    int bytesSent = UdpSockets[communicationChannel].Send(message, message.Length, EndPoints[communicationChannel]);
                }
                catch { }
            }

            private static void InitializeDrone()
            {
                if (!DroneProxy.IsInitialized)
                {
                    DroneController.OnNotifiedTraceMessage("Initializing Drone.", NotificationSource.CommunicationCenter, TraceNotificationLevel.Information);

                    SendMessage(CommunicationChannel.Command, CommandCenter.ComposeCommandString(ATCommands.SetConfiguration, "general:navdata_demo", "TRUE"));

                    int retry = 20;
                    bool bcontinue = true;
                    int next = 0;

                    while (bcontinue && retry > 0)
                    {
                        if (next == 0)
                        {
                            if (DroneProxy.HasCommandReceived)
                            {
                                DroneController.OnNotifiedTraceMessage("Processing 'Initialize Drone Command'.", NotificationSource.DroneController, TraceNotificationLevel.Information);
                                next++;
                            }
                        }
                        else
                        {
                            SendMessage(CommunicationChannel.Command, CommandCenter.ComposeCommandString(ATCommands.SetControlMode, (int)ControlMode.ACK_CONTROL_MODE, 0));                            

                            if (!DroneProxy.HasCommandReceived)
                            {
                                DroneController.OnNotifiedTraceMessage("'Initialize Drone Command' processed successfully.", NotificationSource.DroneController, TraceNotificationLevel.Information);
                                bcontinue = false;
                            }
                        }
                        Thread.Sleep(1000);
                        retry--;
                    }
                }
            }

            #region Thread Methods

            private static void ThreadMethodSendATCommands()
            {
                string commandBatch = null;
                UdpClient socket = UdpSockets[CommunicationChannel.Command];
                IPEndPoint endPoint = new IPEndPoint(IPAddress.Any, 0);

                StopWatch stopWatch = new StopWatch();

                do
                {
                    try
                    {
                        stopWatch.Restart();

                        if (!DroneProxy.IsInitialized)
                        {
                            InitializeDrone();
                        }

                        commandBatch = CommandCenter.GetNextCommandBatch();

                        //Simple way to be able to only view the commands that would be sent and not
                        //actually send them. Maybe to be finetuned at a later stage for some selctivity. 
                        if (!DroneController.DroneControllerConfiguration.EnableATCommandSimulation)
                        {
                            SendMessage(CommunicationChannel.Command, commandBatch);
                        }

                        DroneController.OnNotifiedTraceMessage(commandBatch.Replace("\r", ""), NotificationSource.CommandCenter, TraceNotificationLevel.Verbose);

                        stopWatch.Stop();

                        if (stopWatch.ElapsedMilliseconds < Constants.NavigationDataRefreshInterval)
                        {
                            Thread.Sleep((int)(Constants.NavigationDataRefreshInterval - stopWatch.ElapsedMilliseconds));
                        }

                        if (!WorkerThreadATCommandsActive)
                        {
                            WorkerThreadATCommandsActive = true;
                        }
                    }
                    catch (SocketException socketException)
                    {
                        switch (socketException.ErrorCode)
                        {
                            case 10060: //Socket timeout
                                DroneController.OnNotifiedTraceMessage(String.Format("AT :No activity on worker thread for {0} milliseconds.", Constants.SocketTimeoutNavigationData), NotificationSource.CommunicationCenter, TraceNotificationLevel.Error);
                                WorkerThreadATCommandsActive = false;
                                break;
                        }
                    }
                    catch (Exception exception)
                    {
                        DroneController.OnNotifiedTraceMessage("AT :" + exception.Message, NotificationSource.CommunicationCenter, TraceNotificationLevel.Error);
                    }

                } while (DroneController.ConnectionStatus != ConnectionStatus.Closed);
            }

            private static void ThreadMethodReceiveNavigationData()
            {
                UdpClient socket = UdpSockets[CommunicationChannel.NavigationData];
                IPEndPoint endPoint = new IPEndPoint(IPAddress.Any, 0);

                #region Activate NavigationData Stream

                SendMessage(CommunicationChannel.NavigationData, 1);

                #endregion

                do
                {
                    try
                    {
                        byte[] receiveBuffer = socket.Receive(ref endPoint);

                        if (receiveBuffer.Length == 0)
                        {
                            #region Activate NavigationData Stream

                            SendMessage(CommunicationChannel.NavigationData, 1);

                            #endregion

                            LocalMessageIndex = Constants.DefaultNavigationDataSequence;
                        }
                        else
                        {
                            DroneProxy.NavigationDataBuffer = receiveBuffer;

                            if (DroneProxy.NavigationDataHeader == Constants.NavigationDataHeader)
                            {
                                if (DroneProxy.HasCommunicationProblem)
                                {
                                    LocalMessageIndex = Constants.DefaultNavigationDataSequence;

                                    if (DroneProxy.IsInitialized)
                                    {
                                        SendMessage(CommunicationChannel.Command, CommandCenter.ComposeCommandString(ATCommands.ResetCommunicationHub));
                                        DroneController.OnNotifiedTraceMessage("Sending reset watchdog", NotificationSource.CommunicationCenter, TraceNotificationLevel.Verbose);
                                    }
                                }

                                if (DroneProxy.MessageIndex > LocalMessageIndex)
                                {
                                    DroneProxy.LoadNavigationDataStructures();

                                    if (DroneController.VisionDetectEnabled)
                                    {
                                        //if (DroneProxy.TagCount > 0)
                                        //{
                                        //    Debug.Print(String.Format("Distance: {0}",DroneProxy.TagDistance));
                                        //    Debug.Print(String.Format("BoxWidth: {0}",DroneProxy.TagBoxWidth));
                                        //    Debug.Print(String.Format("BoxHeight: {0}",DroneProxy.TagBoxHeight));
                                        //    Debug.Print(String.Format("Type: {0}",DroneProxy.TagType));
                                        //    Debug.Print(String.Format("X: {0}",DroneProxy.TagX));
                                        //    Debug.Print(String.Format("Y: {0}",DroneProxy.TagY));
                                        //    Debug.Print(new String('=', 20));
                                        //}
                                       
                                        VisionDetectNotificationEventArgs visionDetectNotificationEventArgs = new VisionDetectNotificationEventArgs(DroneProxy.TagCount, DroneProxy.TagType, DroneProxy.TagBoxLeft, DroneProxy.TagBoxTop, DroneProxy.TagBoxWidth, DroneProxy.TagBoxHeight, DroneProxy.TagDistance);
                                        DroneController.OnNotifiedVisionDetect(visionDetectNotificationEventArgs);
                                    }

                                    if (!DroneProxy.ValidateNavigationData())
                                    {
                                        DroneController.OnNotifiedTraceMessage("Navigation data checksum failed. [Preliminary message, might need reviewing.]", NotificationSource.CommunicationCenter, TraceNotificationLevel.Warning);
                                    }
                                }
                                else
                                {
                                    DroneController.OnNotifiedTraceMessage(String.Format("Sequence out of order. Local: {0} - Drone: {1}.[Preliminary message, might need reviewing.]", LocalMessageIndex, DroneProxy.MessageIndex), NotificationSource.CommunicationCenter, TraceNotificationLevel.Warning);
                                }

                                if (!WorkerThreadNavigationDataActive)
                                {
                                    WorkerThreadNavigationDataActive = true;
                                }
                            }

                            LocalMessageIndex = DroneProxy.MessageIndex;
                        }
                    }
                    catch (SocketException socketException)
                    {
                        switch (socketException.ErrorCode)
                        {
                            case 10060: //Socket timeout
                                DroneController.OnNotifiedTraceMessage(String.Format("Nav :No activity on worker thread for {0} milliseconds.", Constants.SocketTimeoutNavigationData), NotificationSource.CommunicationCenter, TraceNotificationLevel.Error);
                                WorkerThreadNavigationDataActive = false;
                                break;
                        }
                    }
                    catch (Exception exception)
                    {
                        DroneController.OnNotifiedTraceMessage("Nav " + exception.Message, NotificationSource.CommunicationCenter, TraceNotificationLevel.Error);
                    }

                } while (DroneController.ConnectionStatus != ConnectionStatus.Closed);
            }

            private static void ThreadMethodReceiveVideoStream()
            {
                UdpClient socket = UdpSockets[CommunicationChannel.VideoStream];
                IPEndPoint remoteEndPoint = EndPoints[CommunicationChannel.VideoStream];

                #region Activate Video Stream

                SendMessage(CommunicationChannel.VideoStream, 1);

                #endregion

                do
                {
                    try
                    {
                        byte[] receiveBuffer = socket.Receive(ref remoteEndPoint);

                        if (receiveBuffer.Length > 0)
                        {
                            DroneController.VideoImage.AddImageStream(receiveBuffer);
                        }

                        if (!WorkerThreadVideoStreamActive)
                        {
                            WorkerThreadVideoStreamActive = true;
                        }
                    }
                    catch (SocketException socketException)
                    {
                        switch (socketException.ErrorCode)
                        {
                            case 10060: //Socket timeout
                                DroneController.OnNotifiedTraceMessage(String.Format("Vid :No activity on worker thread for {0} milliseconds.", Constants.SocketTimeoutNavigationData), NotificationSource.CommunicationCenter, TraceNotificationLevel.Error);
                                WorkerThreadVideoStreamActive = false;
                                break;
                        }
                    }
                    catch (Exception exception)
                    {
                        DroneController.OnNotifiedTraceMessage(exception.Message, NotificationSource.CommunicationCenter, TraceNotificationLevel.Error);
                        WorkerThreadVideoStreamActive = false;
                    }

                } while (DroneController.ConnectionStatus != ConnectionStatus.Closed);
            }

            private static void ThreadMethodReceiveControlInfo()
            {               
                TcpClient       socket          = null;
                NetworkStream   networkStream   = null;
                StringBuilder   stringBuffer    = new StringBuilder();
                byte[]          buffer          = new byte[Constants.ControlInfoBufferSize];

                do
                {
                    try
                    {
                        socket = TcpSockets[CommunicationChannel.ControlInfo];
                        socket.Connect(EndPoints[CommunicationChannel.ControlInfo]);
                        networkStream = socket.GetStream();

                        int byteCount = networkStream.Read(buffer, 0, Constants.ControlInfoBufferSize);

                        if (byteCount > 0)
                        {
                            do
                            {
                                byte[] message = new byte[byteCount];
                                Buffer.BlockCopy(buffer, 0, message, 0, byteCount);
                                stringBuffer.Append(ASCIIEncoding.ASCII.GetString(message));
                                byteCount = 0;
                                if (networkStream.DataAvailable)
                                {
                                    byteCount = networkStream.Read(buffer, 0, Constants.ControlInfoBufferSize);
                                }
                            } while (byteCount > 0);

                            DroneController.UpdateDroneConfigurationValues(stringBuffer.ToString());
                        }
                  
                        if (!WorkerThreadControlInfoActive)
                        {
                            WorkerThreadControlInfoActive = true;
                        }
                    }
                    catch (SocketException socketException)
                    {
                        switch (socketException.ErrorCode)
                        {
                            case 10060: //Socket timeout
                                DroneController.OnNotifiedTraceMessage(String.Format("Ctl :No activity on worker thread for {0} milliseconds.", Constants.SocketTimeoutNavigationData), NotificationSource.CommunicationCenter, TraceNotificationLevel.Error);
                                WorkerThreadControlInfoActive = false;
                                break;
                        }
                    }
                    catch (IOException ioException)
                    {
                        if (ioException.InnerException != null)
                        {
                            SocketException socketException = ioException.InnerException as SocketException;

                            if (socketException != null)
                            {
                                switch (socketException.ErrorCode)
                                {
                                    case 10060: //Socket timeout
                                        DroneController.OnNotifiedTraceMessage(String.Format("Ctl :No activity on worker thread for {0} milliseconds.", Constants.SocketTimeoutNavigationData), NotificationSource.CommunicationCenter, TraceNotificationLevel.Error);
                                        WorkerThreadControlInfoActive = false;
                                        break;
                                }
                            }
                        }
                        else
                        {
                            DroneController.OnNotifiedTraceMessage("Ctl " + ioException.Message, NotificationSource.CommunicationCenter, TraceNotificationLevel.Error);
                        }
                    }
                    catch (Exception exception)
                    {
                        DroneController.OnNotifiedTraceMessage("Ctl " + exception.Message, NotificationSource.CommunicationCenter, TraceNotificationLevel.Error);
                    }
                    finally
                    {
                        stringBuffer.Length = 0;

                        if (networkStream != null)
                        {
                            networkStream.Close();
                        }

                        if (socket != null)
                        {
                            socket.Close();
                        }

                        if (DroneController.ConnectionStatus != ConnectionStatus.Closed)
                        {
                            TcpSockets[CommunicationChannel.ControlInfo] = NetworkHelper.CreateTcpSocket(LocalIPAddress, DroneController.DroneControllerConfiguration.ControlInfoPort, Constants.SocketTimeoutControlInfo);
                        }
                    }

                } while (DroneController.ConnectionStatus != ConnectionStatus.Closed);
            }

            #endregion

            #endregion
        }

        private static class CommandCenter
        {
            #region Private Properties

            private static DroneController DroneController { get; set; }
            private static DroneProxy DroneProxy { get; set; }
            private static Queue<String> Commands { get; set; }
            private static StringBuilder CommandBatch { get; set; }
            private static UInt32 CommandBatchSequence { get; set; }

            #endregion

            #region Construction

            static CommandCenter()
            {
                CommandBatch = new StringBuilder();
                Commands = new Queue<String>();
            }

            #endregion

            #region Private Methods

            private static void EnqueueCommand(string command)
            {
                EnqueueCommand(command, null);
            }

            private static void EnqueueCommand(string command, params object[] parameters)
            {
                Commands.Enqueue(ComposeCommandString(command, parameters));
            }

            #endregion

            #region Internal Methods

            internal static void ResetCommandBatchSequence()
            {
                CommandBatchSequence = 0;
            }

            internal static void RegisterController(DroneController droneController)
            {
                DroneController = droneController;
                DroneProxy = droneController.DroneProxy;
            }

            internal static string ComposeCommandString(string command, params object[] parameters)
            {
                if (parameters == null)
                {
                    return String.Format(command, CommandBatchSequence++);
                }
                else
                {
                    List<object> list = new List<object>(parameters);
                    list.Insert(0, CommandBatchSequence++);        
                    return String.Format(command, list.ToArray()); 
                }
            }

            /// <summary>
            /// This method dequeues all messages currently in the queue and concatenates them.
            /// The sequence is automatically added at the beginning of the string.
            /// </summary>
            /// <returns>A string containing a chain of AT commands.</returns>
            /// <remarks>
            /// I do not currently take into account the 1024 character limit of
            /// a concatenated command string. Testing will indicate whether it is 
            /// possible that the string becomes larger than the limit between 
            /// two sweeps of the command queue. If so I then take measures to cope.
            /// </remarks>
            internal static string GetNextCommandBatch()
            {
                CommandBatch.Length = 0;

                int commandCount = Commands.Count;

                #region Add CommandQueue Commands

                if (commandCount > 0)
                {
                    for (int commandIndex = 0; commandIndex < commandCount; commandIndex++)
                    {
                        CommandBatch.Append(Commands.Dequeue());
                    }
                }

                #endregion

                #region Add Sequence & InputValue

                CommandBatch.Append(ComposeCommandString(ATCommands.SetInputValue, (uint)DroneProxy.DroneInputValue));

                #endregion

                return CommandBatch.ToString();
            }

            internal static void SwitchVideoChannel(VideoChannel videoChannel)
            {
                EnqueueCommand(ATCommands.SwitchVideoChannel, (int)videoChannel);
            }

            internal static void SetProgressiveInputValues(float roll, float pitch, float height, float yaw)
            {
                int newPitch = 0;
                int newRoll = 0;
                int newGaz = 0;
                int newYaw = 0;

                roll = (Math.Abs(roll) > 1) ? 1 : roll;
                pitch = (Math.Abs(pitch) > 1) ? 1 : pitch;
                height = (Math.Abs(height) > 1) ? 1 : height;
                yaw = (Math.Abs(yaw) > 1) ? 1 : yaw;

                unsafe
                {
                    float value = 0;

                    value = pitch;
                    newPitch = *(int*)(&value);

                    value = roll;
                    newRoll = *(int*)(&value);

                    value = height;
                    newGaz = *(int*)(&value);

                    value = yaw;
                    newYaw = *(int*)(&value);

                }

                EnqueueCommand(ATCommands.SetProgressiveInputValues, 1, newRoll, newPitch, newGaz, newYaw);
            }

            internal static void SetFlatTrim()
            {
                EnqueueCommand(ATCommands.SetFlatTrim);
            }

            internal static void StartVisonDetect()
            {
                int newValue;
                unsafe
                {
                    float value = 0;
                    newValue = *(int*)(&value);
                }
                EnqueueCommand(ATCommands.SetTagDetection, (int)DetectionType.CAD_TYPE_VISION_DETECT, newValue);
            }

            internal static void StopVisonDetect()
            {
                int newValue;
                unsafe
                {
                    float value = 0;
                    newValue = *(int*)(&value);
                }
                EnqueueCommand(ATCommands.SetTagDetection, (int)DetectionType.CAD_TYPE_NONE, newValue);
            }

            internal static void SetConfiguration(string parameterName, string parameterValue)
            {
                EnqueueCommand(ATCommands.SetConfiguration, parameterName, parameterValue);
            }

            internal static void SetControlMode(ControlMode controlMode)
            {
                EnqueueCommand(ATCommands.SetControlMode, controlMode, 0);
            }

            internal static void SetControlMode(ControlMode controlMode, int length)
            {
                EnqueueCommand(ATCommands.SetControlMode, controlMode, length);
            }

            internal static void ResetCommunicationHub()
            {
                EnqueueCommand(ATCommands.ResetCommunicationHub);
            }

            internal static void PlayLedAnimation(LedAnimation ledAnimation, int frequency, int duration)
            {
                int newFrequency;
                unsafe
                {
                    float value = frequency;
                    newFrequency = *(int*)(&value);
                }
                EnqueueCommand(ATCommands.PlayLedAnimation, (int)ledAnimation, newFrequency, duration);
            }

            internal static void RequestConfiguredValues()
            {
                EnqueueCommand(ATCommands.SetControlMode, ControlMode.CFG_GET_CONTROL_MODE, 0);
            }

            #endregion
        }

        #endregion
    }
}
